#ifndef PHIDGET_ALREADY
#define PHIDGET_ALREADY
#include "phidget21.h"
#endif


#define ELBOW_VELOCITY 512.0

#define COUNTS_PER_STEP 16.0
#define DEGREES_PER_STEP 1.8
#define GEAR_RATIO 15.0


class ElbowController {

public:
	CPhidgetStepperHandle stepper;

	// initialize the velocity etc.
	void init(CPhidgetStepperHandle stepperIn) {
		stepper = stepperIn;
		CPhidgetStepper_setVelocityLimit(stepper, 0, ELBOW_VELOCITY);
	}

	// set the target position to the current position so it doesn't move when we engage
	void engage() {
		long long pos;
		CPhidgetStepper_getCurrentPosition(stepper, 0, &pos);
		CPhidgetStepper_setCurrentPosition(stepper, 0, pos);
		CPhidgetStepper_setEngaged(stepper, 0, 1);
	}

	// set current point as zero
	void setCurrentAsHome() {
		CPhidgetStepper_setCurrentPosition(stepper, 0, 0);
	}

	// raw counts are 16 per full step
	void setPositionByCount(long long count) {
		CPhidgetStepper_setTargetPosition(stepper, 0, count);
	}
	long long getPositionByCount() {
		long long pos;
		CPhidgetStepper_getCurrentPosition(stepper, 0, &pos);
		return pos;
	}

	// full steps
	void setPositionBySteps(long long steps) {
		setPositionByCount(steps * COUNTS_PER_STEP);
	}
	double getPositionBySteps() {
		return getPositionByCount() / COUNTS_PER_STEP;
	}

	// degrees
	void setPositionByDegrees(double degrees) {
		setPositionBySteps((long long)(degrees / DEGREES_PER_STEP));
	}
	double getPositionByDegrees() {
		return getPositionBySteps() * DEGREES_PER_STEP;
	}

	// to move the capstan, we have to include the gear ratio
	void setCapstanPositionByDegrees(double degrees) {
		setPositionBySteps((long long)(degrees * GEAR_RATIO / DEGREES_PER_STEP));
	}
	double getCapstanPositionByDegrees() {
		return getPositionBySteps() * DEGREES_PER_STEP / GEAR_RATIO;
	}
	
	void disengage() {
		CPhidgetStepper_setEngaged(stepper, 0, 0);
	}
};